# -*- coding:utf-8 -*-
# @FileName :double_line_change.py
# @Time :2024/5/3 10:21
# @Author :ShengYe
# @Des :
import carla
import math
import numpy as np
import matplotlib.pyplot as plt

from my_controller.controller import Vehicle_control
from planner import planning_utils
from my_utils import generate_dlc_path

class CarlaWorld:
    def __init__(self, ego_spawn_point_index=7):
        self.client = carla.Client("localhost", 2000)
        self.client.set_timeout(10)
        self.world = self.client.load_world('town05')
        self.amap = self.world.get_map()

        all_spawn_points = self.amap.get_spawn_points()
        self.ego_spawn_point = all_spawn_points[ego_spawn_point_index]
        self.ego_vehicle = self.spawn_ego_vehicle()

    def spawn_ego_vehicle(self):
        model3_bp = self.world.get_blueprint_library().find('vehicle.tesla.model3')
        model3_bp.set_attribute('color', '255,88,0')
        model3_actor = self.world.spawn_actor(model3_bp, self.ego_spawn_point)

        print("【车辆已经生成】transform为", self.ego_spawn_point)
        physics_control = carla.VehiclePhysicsControl()
        physics_control.mass = 1412
        model3_actor.apply_physics_control(physics_control)
        return model3_actor

    def get_ego_vehicle_info(self):
        physics_control = self.ego_vehicle.get_physics_control()
        wheel1 = physics_control.wheels[1].position
        wheel3 = physics_control.wheels[3].position
        dis13 = math.sqrt((wheel1.x - wheel3.x) ** 2 + (wheel1.y - wheel3.y) ** 2)
        wheelbase = dis13 / 100
        lr = lf = wheelbase / 2
        mass = physics_control.mass
        vehicle_para = (lr, lf, -110000, -110000, mass, 1537)
        return vehicle_para

    def get_controller(self, path):
        return Vehicle_control(
            ego_vehicle=self.ego_vehicle,
            vehicle_para=self.get_ego_vehicle_info(),
            pathway=path,
            controller_type="MPC_controller"
        )

    def set_spectator(self):
        spectator = self.world.get_spectator()
        transform = self.ego_vehicle.get_transform()
        spectator.set_transform(
            carla.Transform(transform.location + carla.Location(z=50), carla.Rotation(pitch=-90))
        )

    def get_lat_acc(self):
        acc = self.ego_vehicle.get_acceleration()
        acc_vec = np.array([acc.x, acc.y, acc.z])
        right_vec = self.ego_vehicle.get_transform().get_right_vector()
        right_vec = np.array([right_vec.x, right_vec.y, right_vec.z])
        lat_acc = np.dot(acc_vec, right_vec)
        return lat_acc

    def get_velocity(self):
        velocity = self.ego_vehicle.get_velocity()
        return 3.6 * math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)

    def get_direction(self):
        return self.ego_vehicle.get_transform().rotation.yaw * (math.pi / 180)

    def get_distance(self, path):
        location = self.ego_vehicle.get_transform().location
        return location.distance(path[-1][0].transform.location)

    def emergency_brake(self):
        brake_control = carla.VehicleControl()
        brake_control.steer = 0
        brake_control.throttle = 0
        brake_control.brake = 1
        self.ego_vehicle.apply_control(brake_control)

    def clean(self):
        if self.ego_vehicle is not None:
            self.ego_vehicle.destroy()
        settings = self.world.get_settings()
        settings.synchronous_mode = False
        settings.fixed_delta_seconds = None


class Draw:
    def __init__(self):
        self.direction = []
        self.speed = []
        self.target_speed = []
        self.lat_acc = []
        self.car_position = []
        self.pathway = []

    def update(self, carla_world, max_speed):
        self.lat_acc.append(carla_world.get_lat_acc())
        self.direction.append(carla_world.get_direction())
        self.speed.append(carla_world.get_velocity())
        self.target_speed.append(max_speed)
        self.car_position.append([carla_world.ego_vehicle.get_location().x, carla_world.ego_vehicle.get_location().y])

    def draw_waypoints(self, waypoints, debug):
        for waypoint in waypoints:
            debug.draw_point(
                waypoint[0].transform.location,
                size=0.05,
                color=carla.Color(0, 255, 0),
                life_time=0
            )

    def show(self):
        plt.subplot(2, 2, 1)
        plt.plot(self.direction)
        plt.ylim(-5, 5)
        plt.title('delta')

        plt.subplot(2, 2, 2)
        plt.plot(self.speed)
        plt.plot(self.target_speed, color="r")
        plt.ylim(0, max(self.target_speed) + 10)
        plt.title('speed')

        plt.subplot(2, 2, 3)
        x1 = [point[0] for point in self.car_position]
        y1 = [point[1] for point in self.car_position]
        plt.plot(x1, y1, label='real', marker='o', alpha=0.5, markersize=1, linestyle='-', color='blue')
        x2 = [point.x for point in self.pathway]
        y2 = [point.y for point in self.pathway]
        plt.plot(x2, y2, label='refer', marker='s', alpha=0.5, markersize=1, linestyle='-', color='orange')
        plt.legend()
        plt.title("path")

        plt.subplot(2, 2, 4)
        plt.plot(self.lat_acc)
        plt.title('l_acc')
        plt.show()


def main(carla_world, controller, draw, waypoints, max_speed=50):
    try:
        carla_world.set_spectator()

        while True:
            carla_world.world.tick()
            carla_world.set_spectator()
            control = controller.run_step(target_speed=max_speed)

            debug = carla_world.world.debug
            debug.draw_point(
                carla.Location(controller.Lat_control.x_pre, controller.Lat_control.y_pre, 2),
                size=0.05,
                color=carla.Color(0, 255, 255),
                life_time=0
            )
            debug.draw_point(
                carla.Location(controller.Lat_control.x_pro, controller.Lat_control.y_pro, 2),
                size=0.05,
                color=carla.Color(100, 0, 0),
                life_time=0
            )

            carla_world.ego_vehicle.apply_control(control)
            draw.update(carla_world, max_speed)
            dist = carla_world.get_distance(waypoints)

            if dist < 2:
                carla_world.emergency_brake()
                print("【车辆到达终点】transform为", carla_world.ego_vehicle.get_transform())
                break

    finally:
        carla_world.clean()


if __name__ == "__main__":
    carla_world = CarlaWorld()
    draw = Draw()
    debug = carla_world.world.debug

    waypoints = generate_dlc_path.get_waypoints_2(carla_world.ego_spawn_point)
    draw.pathway = [obj[0].transform.location for obj in waypoints]
    draw.draw_waypoints(waypoints, debug)

    global_frenet_path = planning_utils.waypoint_list_2_target_path(waypoints)
    controller = carla_world.get_controller(global_frenet_path)

    main(carla_world, controller, draw, waypoints)
    draw.show()
